#include <iostream>
#include <fcscomm.h>
#include <buffer.h>
#include <properties.h>
#include <crc.h>
#include <stdint.h>
#include <protocol.h>
#include <../SerialPort/serialport.h>


const int FcsComm::datasize_ = 256;
const uint8_t FcsComm::addr_ = 0xF0;
const uint8_t FcsComm::id_ = 0;
const uint8_t FcsComm::dbg_id_ = 0xDE;


FcsComm::FcsComm(void)
	: fcs_(NULL)
	, data_(NULL)
{
	data_ = new uint8_t[datasize_];
}


FcsComm::~FcsComm()
{
	if (data_ != NULL)
		delete [] data_;
}


void FcsComm::setInterface(SerialPort *fcs)
{
	fcs_ = fcs;
}


bool FcsComm::doCycle(void)
{
	Protocol protocol(fcs_, data_, datasize_);

	uint8_t addr = 0;
	uint8_t id = 0;
	int size = 0;

	if (!protocol.receive(addr, id, size))
	{
		fcs_->clean();
		return false;
	}

	if (addr == addr_)
	{
		if (id == dbg_id_)
		{
			printDebugInfo(protocol.getDataPointer(), size);
			return true;
		}
		else if (id == id_)
		{
			fcs_->clean();
			request(protocol.getDataPointer(), size);
			protocol.send(addr_, id_ + 1, response(protocol.getDataPointer(), protocol.getMaxDataSize()));
		}
	}

	return true;
}


void FcsComm::request(uint8_t *data, int datasize)
{
	PropAccessor p;

	Buffer buf(data, datasize);
	p().axis[0] = buf.getFloat32();	// aileron
	p().axis[1] = -buf.getFloat32();	// elevator
	p().axis[2] = buf.getFloat32();	// rudder
	p().axis[3] = buf.getFloat32();	// throttle
	p().axis[4] = buf.getFloat32();	// flaps
	p().axis[5] = buf.getFloat32();	// brakes
	p().axis[6] = buf.getFloat32();	// parachute
	p().axis[7] = static_cast<int8_t>(buf.getUint8()) / 127.0f;	// azimuth speed
	p().axis[8] = static_cast<int8_t>(buf.getUint8()) / 127.0f;	// pitch speed
	p().axis[9] = static_cast<int8_t>(buf.getUint8()) / 127.0f;	// zoom speed
}


int FcsComm::response(uint8_t *data, int datasize)
{
	PropAccessor p;
	
	Buffer buf(data, datasize);
	buf.putFloat32(p().orientation.roll);
	buf.putFloat32(p().orientation.pitch);
	buf.putFloat32(p().orientation.heading);
	buf.putFloat32(p().orientation.roll_rate);
	buf.putFloat32(p().orientation.pitch_rate);
	buf.putFloat32(p().orientation.yaw_rate);
	buf.putFloat32(p().orientation.accel_x);
	buf.putFloat32(p().orientation.accel_y);
	buf.putFloat32(p().orientation.accel_z);

	buf.putFloat32(p().air_data.airspeed);
	buf.putFloat32(p().air_data.altitude);
	buf.putFloat32(p().air_data.v_speed);
	
	buf.putFloat32(p().directalt.altitude);

	buf.putFloat64(p().position.latitude);
	buf.putFloat64(p().position.longitude);
	buf.putFloat32(p().position.altitude);
	buf.putFloat32(p().position.ground_speed);
	buf.putFloat32(p().position.course);

	buf.putUint16(p().utc_time.year);
	buf.putUint8(p().utc_time.month);
	buf.putUint8(p().utc_time.day);
	buf.putUint8(p().utc_time.hours);
	buf.putUint8(p().utc_time.minutes);
	buf.putUint8(p().utc_time.seconds);

	buf.putUint16(p().engine.rpm);
	buf.putUint16(p().engine.temp);
	buf.putUint8(p().engine.fuel_level);

	buf.putUint8(p().gearonground ? 1 : 0);

	buf.putFloat32(p().angles.aoa);
	buf.putFloat32(p().angles.slip);

	buf.putUint16(static_cast<int16_t>(p().ois.azimuth * 100.0f));
	buf.putUint16(static_cast<int16_t>(p().ois.elevation * 100.0f));
	buf.putUint16(static_cast<int16_t>(p().ois.zoom * 100.0f));

	buf.putUint16(p().failures);

	buf.putFloat64(p().air_data.static_pressure);
	buf.putFloat64(p().air_data.total_pressure - p().air_data.static_pressure);
	buf.putFloat32(p().air_data.temp);

	return buf.getIndexForPut();
}


void FcsComm::printDebugInfo(uint8_t *data, int datasize)
{
	data[datasize] = 0;
	std::cout << data;
}



